#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  none,     none,     none)
#pragma config(Hubs,  S3, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     DriveFL,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     DriveBL,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     LiftL,         tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     DriveFR,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     DriveBR,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S3_C1_1,     LiftR,         tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S3_C1_2,     Roller,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S3_C2_1,    TubeG,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_2,    LDoor,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_3,    RDoor,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_4,    SensorServo,          tServoStandard)
#pragma config(Servo,  srvo_S3_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S3_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

//task lift()

 //while(nMotorEncoder[LiftR] < 1675)
  // {
  // 	motor[LiftR] = 100;
	//	motor[LiftL] = 100;

	//	}

	//	motor[LiftR] = 15;
	//	motor[LiftL] = 15;

  // servo[TubeG] = 255;
  // stoptask lift()

//Defining all necessary Variables.

bool LiftUp;
bool TubeGUp;
bool DoorBtnPress;

void RstLiftEnc(){
	nMotorEncoder[LiftR] = 0;
	nMotorEncoder[LiftL] = 0;
	wait1msec(100);
}

void RstMtrEnc(){
	nMotorEncoder[DriveFR] = 0;
	nMotorEncoder[DriveFL] = 0;
	nMotorEncoder[DriveBR] = 0;
	nMotorEncoder[DriveLL] = 0;
	wait1msec(100);
}
void InitializeRobot(){
	motor[LiftR] = 1;
	motor[LiftL] = 1;
	RstMtrEnc();
	RstLiftEnc();
	TubeGUp = false;
	LiftUp = false;
	CurMtrEnc = 0
	DoorBtnPress = false;
	servo[LDoor] = 0;
	servo[RDoor] = 256;
	servo[TubeG] = 186;
	servo[SensorServo] = 20;
	wait1msec(100);
}

void DrvStr (float distance, int power, string direction){
	if direction == "Rev" {dir == -1;} else {dir==1;}
	target == distance * 111;
	motor[DriveFR] = power*dir;
	motor[DriveFL] = power*dir;
	motor[DriveBR] = power*dir;
	motor[DriveBL] = power*dir;
	while (CurMtrEnc*dir < target {
		CurMtrEnc == (nMotorEncoder [DriveFR] + nMotorEncoder [DriveFL]
		+ nMotorEncoder [DriveBR] + nMotorEncoder [DriveBL])/4;
	}
}

void FullPower(string DirFull)
	if DirFull == "Rev" {dir == -1;} else {dir==1;}
	motor[DriveFR] = 100*dir;
	motor[DriveBR] = 100*dir;
	motor[DriveFL] = 100*dir;
	motor[DriveBL] = 100*dir;

void StopMtr () {
	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;
}

GrbHgt =
LowHgt =
MedHgt = 1650
TallHgt =
CtrHgt =
CurLftEnc = 0


void RaiseLift (int Hgt, int LiftPower) {
	CurLftEnc = 0
	motor[LiftR] = LiftPower;
	motor[LiftL] = LiftPoser;
	while(CurLftEnc < Hgt) {
		CurLftEnc = (nMotorEncoder[LiftR] + nMotorEncoder[LiftL])/2
	}
		motor[LiftR] = 15;
		motor[LiftL] = 15;

void PreGrb () {
	RaiseLift(GrbHgt, 100);
	servo[TubeG] = 255;
}

void DrpBall (string Doors){
	if doors = "right" {servo[RDoor] = 172;}
	if doors = "left" {servo[LDoor] = 65;}
	else {servo[RDoor] = 172; servo[LDoor] = 65; }





task main(){

	InitializeRobot();
	waitForStart();
	DrvStr(6,30,Rev);
	FullPower(Rev);
	RaiseLift(MedHgt,50);
	DrvStr(78,100,Rev);
	DrvStr(82,50,Rev);
	DrvStr(84,25,Rev);
	StopMtr();
	RstMtrEnc();
	DropBall(Right)


}



servo[RDoor] = 172;

wait1Msec(1100);

servo[TubeG] = 186;

while (nMotorEncoder[LiftR] > 800)

			{

					motor[DriveFR] = 0;
					motor[DriveFL] = 0;
					motor[DriveBR] = 0;
					motor[DriveBL] = 0;


					motor[LiftR] = 1;
					motor[LiftL] = 1;

			}

		while(nMotorEncoder[LiftR] >= 50)

			{

			motor[DriveFR] = 0;
			motor[DriveFL] = 0;
			motor[DriveBR] = 0;
			motor[DriveBL] = 0;


			motor[LiftR] = -1;
			motor[LiftL] = -1;

			}

		motor[LiftR] = 0;
		motor[LiftL] = 0;

		nMotorEncoder[LiftR] = 0;
		nMotorEncoder[LiftL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;


while (nMotorEncoder [DriveFR] > -750)

	{

	motor[DriveFR] = -100;
	motor[DriveBR] = -100;

	motor[DriveFL] = -100;
	motor[DriveBL] = -100;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;



while (nMotorEncoder [DriveFR] < 850)

	{

	motor[DriveFR] = 100;
	motor[DriveBR] = 100;

	motor[DriveFL] = 100;
	motor[DriveBL] = 100;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;

wait1Msec(500);


while (nMotorEncoder [DriveFR] < 3800)
	{

	motor[DriveFR] = 70;
	motor[DriveBR] = 70;

	motor[DriveFL] = -70;
	motor[DriveBL] = -70;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;


while (nMotorEncoder [DriveFR] > (-87 * 101) - 4)

	{

	motor[DriveFR] = -15;
	motor[DriveFL] = -100;
	motor[DriveBR] = -15;
	motor[DriveBL] = -100;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[LiftR] = 0;

nMotorEncoder[DriveFR] = 0;

while (nMotorEncoder [DriveFR] < 50)
	{

	motor[DriveFR] = 30;
	motor[DriveBR] = 30;

	motor[DriveFL] = -100;

	motor[DriveBL] = -100;
	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;


	while(nMotorEncoder[LiftR] < 500)
		{

		motor[LiftR] = 100;
		motor[LiftL] = 100;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;


	servo[TubeG] = 255;

	servo[RDoor] = 256;


while (nMotorEncoder [DriveFR] > -300)
	{

	motor[DriveFR] = -100;
	motor[DriveBR] = -100;

	motor[DriveFL] = 100;

	motor[DriveBL] = 100;
	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFL] = 0;

nMotorEncoder[DriveFR] = 0;

while (nMotorEncoder [DriveFR] < (85 * 101) + 4)

	{

	motor[DriveFR] = 100;
	motor[DriveFL] = 100;
	motor[DriveBR] = 100;
	motor[DriveBL] = 100;

	}


	motor[DriveFR] = 30;
	motor[DriveFL] = 30;
	motor[DriveBR] = 30;
	motor[DriveBL] = 30;

	wait1Msec(1000);

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFL] = 0;

while (nMotorEncoder [DriveFL] < 4100)

	{

	motor[DriveFR] = -70;
	motor[DriveBR] = -70;

	motor[DriveFL] = 70;
	motor[DriveBL] = 70;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;



while (nMotorEncoder [DriveFR] > -2000)

	{

	motor[DriveFR] = -100;
	motor[DriveBR] = -100;

	motor[DriveFL] = -100;
	motor[DriveBL] = -100;

	}

	motor[DriveFR] = 0;
	motor[DriveFL] = 0;
	motor[DriveBR] = 0;
	motor[DriveBL] = 0;

nMotorEncoder[DriveFR] = 0;
nMotorEncoder[DriveFL] = 0;



while(nMotorEncoder[LiftR] < 2600)
		{

		motor[LiftR] = 100;
		motor[LiftL] = 100;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;

servo[LDoor] = 65;

wait1Msec(350);

	motor[DriveFR] = -100;
	motor[DriveBR] = -100;

	motor[DriveFL] = -100;
	motor[DriveBL] = -100;


wait1Msec(650);


	motor[DriveFR] = 0;
	motor[DriveBR] = 0;

	motor[DriveFL] = 0;
	motor[DriveBL] = 0;

servo[LDoor] = 0;

servo[TubeG] = 186;

wait1Msec(1500);

}
